#pragma once
#include <cstdint>
#include <array>

/**
 * @example
 * cout << bitset<64>(concat64(0b1,0b11).val) << endl;
 * // outputs: 0000000000000000000000000000000100000000000000000000000000000011
 * cout << bitset<64>(concat64(0b1,0b11,0b111,0b1111).val) << endl;
 * // outputs: 0000000000010000000000000011000000000000011100000000000011110000
 */
union concat64
{
  concat64(): val(0) {};
  concat64(uint32_t part0, uint32_t part1){
    cat2.part1=part1;
    cat2.part0=part0;
  };
  concat64(uint16_t part0, uint16_t part1, uint16_t part2, uint16_t part3)
  {
    cat4.part3=part3;
    cat4.part2=part2;
    cat4.part1=part1;
    cat4.part0=part0;
  }

  uint64_t val;
  struct cat2 {
    uint32_t part1;
    uint32_t part0;
  } cat2;
  struct cat4 {
    uint16_t part3;
    uint16_t part2;
    uint16_t part1;
    uint16_t part0;
  } cat4;
};

/**
 * out[0] = in[1][0]
 */
template<uint32_t in_len, uint32_t out_len = in_len/2>
std::array<uint64_t, out_len> concat32x2(const std::array<uint32_t, in_len>& in_array)
{
  static_assert(in_len%2 == 0);
  std::array<uint64_t, out_len> out_array;

  for (size_t i=0; i<out_len; ++i) {
    out_array[i] = concat64(in_array[i*2+1], in_array[i*2]).val;
  }

  return out_array;
}

/**
 * out[0] = in[3][2][1][0]
 */
template<uint32_t in_len, uint32_t out_len = in_len/4>
std::array<uint64_t, out_len> concat16x4(const std::array<uint16_t, in_len>& in_array)
{
  static_assert(in_len%4 != 0);
  std::array<uint64_t, out_len> out_array;

  for (size_t i=0; i<out_len; ++i) {
    out_array[i] = concat64(in_array[i*4+3], in_array[i*4+2], in_array[i*4+1], in_array[i*4]);
  }

  return out_array;
}

/**
 * @example
 * long2double(2, 3)   
 *  integer 2: 0b0 010 
 *                ↑    
 *               dot=3 
 *  return val: 0.25   
 * 
 */
constexpr double long2double(uint64_t integer, uint8_t dot)
{
  bool is_neg = ((uint64_t)1<<63) & integer;
  uint64_t val_pos = is_neg? ~integer+1: integer;
  uint64_t float_mask = ~((uint64_t)-1 << dot);
  uint64_t part_float = val_pos & float_mask;
  uint64_t part_int = val_pos >> dot;

  double val = 0.0f;
  for (uint8_t i=0; i<dot; ++i) {
    val = (val + (part_float & 1)) * 0.5;
    part_float = part_float >> 1;
  }
  val += part_int;
  val = is_neg? val*-1: val;

  return val;
}
